﻿
using System;
using System.Collections.Generic;
using System.Linq;
using PS.DataTypes;

namespace PS.Algorithms
{
    /// <summary>
    /// Algorithms to find paths.
    /// </summary>
    public static class Pathfinder
    {
        public static IConnection[] PathDijkstra(IGraph graph, int start, int goal)
        {
            // Initialize a record for the start node
            NodeRecord startNodeRecord = new NodeRecord { Node = start, Connection = null, CostSoFar = 0 };

            // Initialize the open and closed lists
            NodeRecord[] openNodes = new NodeRecord[goal + 1];
            openNodes[startNodeRecord.Node] = startNodeRecord;
            NodeRecord[] closedNodes = new NodeRecord[goal + 1];

            NodeRecord currentNodeRecord = null;

            int openNodesCount = goal;

            // Process each open node
            while (openNodesCount > 0)
            {
                openNodesCount--;
                currentNodeRecord = openNodes.SmallestElement();
                int currentNode = currentNodeRecord.Node;

                // If its the goal node then terminate
                if (currentNode == goal) break;

                IEnumerable<IConnection> connections = graph.Connections(currentNode);

                foreach (IConnection connection in connections)
                {
                    int endNode = connection.ToNode;

                    // Skip if the node is closed
                    if (closedNodes[endNode] != null) continue;

                    float endNodeCost = currentNodeRecord.CostSoFar + connection.Cost;
                    NodeRecord endNodeRecord = new NodeRecord();

                    if (openNodes[endNode] != null)
                    {
                        endNodeRecord = openNodes[endNode];
                        if (endNodeRecord.CostSoFar <= endNodeCost) continue;
                    }
                    else
                    {
                        endNodeRecord.Node = endNode;
                    }

                    endNodeRecord.CostSoFar = endNodeCost;
                    endNodeRecord.Connection = connection;
                    openNodes[endNode] = endNodeRecord;
                }

                // We’ve finished looking at the connections for the current node, so add it to the closed list and remove it from the open list
                openNodes[currentNode] = null;
                closedNodes[currentNode] = currentNodeRecord;
            }

            IConnection[] path = null;

            if (currentNodeRecord != null && currentNodeRecord.Node == goal)
            {
                path = new IConnection[graph.Connections().Length];

                int i = 0;
                while (currentNodeRecord.Node != start)
                {
                    path[i] = currentNodeRecord.Connection;
                    currentNodeRecord = closedNodes[currentNodeRecord.Connection.FromNode];
                    i++;
                }

                IConnection[] pathResized = (from IConnection c in path where c != null select c).ToArray();

                // We need the path in the form A => B => C, not C => B => A
                Array.Reverse(pathResized);
                path = pathResized;
            }

            return path;
        }
    }
}
